//-----------------------------------------------------------------------
//  This file is part of the Microsoft Robotics Studio Code Samples.
//
//  Copyright (C) Microsoft Corporation.  All rights reserved.
//
//  $File: BSMotor.cs $ $Revision: 21 $
//-----------------------------------------------------------------------

using System;
using System.Net;
using System.Collections.Generic;
using System.Security.Permissions;

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.Core.DsspHttp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;

using boe = Microsoft.Robotics.Services.BasicStamp2.Proxy;
using coord = Microsoft.Robotics.Services.Coordination.Proxy;
using motor = Microsoft.Robotics.Services.Motor.Proxy;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using System.ComponentModel;

namespace Microsoft.Robotics.Services.BasicStamp2.Motor
{
    
    [Contract(Contract.Identifier)]
    [AlternateContract(motor.Contract.Identifier)]
    [DisplayName("Boe-Bot Generic Motor")]
    [Description("Provides access to the Parallax BASIC Stamp 2 Boe-Bot motor.\n(Uses the Generic Motor contract.)")]
    public class MotorService : DsspServiceBase
    {
        [EmbeddedResource("Microsoft.Robotics.Services.BasicStamp2.BoeBotMotor.xslt")]
        string _transform = null;
        
        [InitialStatePartner(Optional=true)]
        private motor.MotorState _state = new motor.MotorState();

        [ServicePort("/BSMotor", AllowMultipleInstances=true)]
        private motor.MotorOperations _mainPort = new motor.MotorOperations();

        [Partner("BASICStamp2", Contract=boe.Contract.Identifier, CreationPolicy=PartnerCreationPolicy.UseExistingOrCreate, Optional=false)]
        private boe.BasicStamp2Operations _boebotPort = new boe.BasicStamp2Operations();

        [Partner("SubMgr", Contract=submgr.Contract.Identifier, CreationPolicy=PartnerCreationPolicy.CreateAlways, Optional=false)]
        private submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();

        public MotorService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
			
        }

        protected override void Start()
        {
            InitializeState();

            // call base Start() method that will activate operation handlers, log uri, insert to directory
            base.Start();
        }

        /// <summary>
        /// Initialize default state
        /// </summary>
        private void InitializeState()
        {
            
            if (_state == null)
            {
                _state = new motor.MotorState();
                _state.PowerScalingFactor = 1.0;
                _state.HardwareIdentifier = 1;
                _state.Name = "Left";
                _state.CurrentPower = 0.0;
                SaveState(_state);
            }
        }

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual void GetHandler(motor.Get get)
        {
            get.ResponsePort.Post(_state);
        }

        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(HttpGet httpGet)
        {
            httpGet.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _state,
                _transform)
            );
            yield break;
        }

        /// <summary>
        /// Replace Handler
        /// </summary>
        /// <param name="replace"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual void ReplaceHandler(motor.Replace replace)
        {
            _state = replace.Body;
            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
        }


        /// <summary>
        /// SetMotorPower Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetMotorPowerHandler(motor.SetMotorPower update)
        {
            boe.MotorSpeed motorSpeed = new boe.MotorSpeed();

            // Default -1 which is ignored by the controller.
            motorSpeed.LeftSpeed = null;
            motorSpeed.RightSpeed = null;

            if (_state.HardwareIdentifier == 1)
                motorSpeed.LeftSpeed = update.Body.TargetPower;
            else
                motorSpeed.RightSpeed = update.Body.TargetPower;

            coord.ActuatorCoordination coordination = update.GetHeader<coord.ActuatorCoordination>();
            if (coordination == null)
            {
                _boebotPort.UpdateMotorSpeed(motorSpeed);
            }
            else
            {
                // Pass on the coordination to the Boe-Bot
                boe.UpdateMotorSpeed updateMotorSpeed = new boe.UpdateMotorSpeed(motorSpeed);
                updateMotorSpeed.AddHeader(coordination);
                _boebotPort.Post(updateMotorSpeed);
            }

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }


    }

    /// <summary>
    /// The BasicStamp2 Motor Contract
    /// </summary>
    public static class Contract
    {
        // TT - Modified the Contract Identifier for new version
        public const string Identifier = "http://schemas.microsoft.com/robotics/2007/10/bsmotor.html";
    }
}
